import tkinter as tk
from PIL import Image, ImageDraw
import json
import math
import asyncio
import threading
import websockets
import math

class MapViewer:
    def __init__(self, root, file_path, center_utm, scale, canvas_size, base_station_utm):
        self.file_path = file_path
        self.center_utm = center_utm
        self.scale = scale
        self.canvas_size = canvas_size
        self.center_pixel = (canvas_size[0] // 2, canvas_size[1] // 2)
        self.offset = [0, 0]  # Offset for panning
        self.zoom_factor = 1.0  # Zoom factor
        self.car_utm = (444460.68, 4427003.31)  # Initial car position (Easting, Northing)
        self.base_station_utm = base_station_utm
        self.car_yaw = 0  # Initial car yaw angle (in degrees)

        # Initialize tkinter canvas
        self.canvas = tk.Canvas(root, width=canvas_size[0], height=canvas_size[1], bg="white")
        self.canvas.pack(fill=tk.BOTH, expand=True)
        self.canvas.bind("<ButtonPress-3>", self.start_pan)
        self.canvas.bind("<B3-Motion>", self.pan_map)
        self.canvas.bind("<MouseWheel>", self.zoom_map)
        self.canvas.bind("<Button-1>", self.on_click)  # Left-click binding to get UTM coordinates
        # Read and process data
        coords = self.read_coordinates()
        self.relative_coords = self.convert_to_relative(coords)

        # Draw initial map
        self.draw_map()

    def read_coordinates(self):
        """Read coordinates from a text file."""
        with open(self.file_path, 'r') as file:
            lines = file.readlines()
        coordinates = []
        for line in lines:
            if line.strip():
                points = line.strip().split(',')
                coordinates.append([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])
        return coordinates

    def convert_to_relative(self, coords):
        """Convert UTM coordinates to relative pixel coordinates."""
        relative_coords = []
        for segment in coords:
            relative_segment = []
            for x, y in segment:
                rel_x = (x - self.center_utm[0]) / self.scale
                rel_y = (self.center_utm[1] - y) / self.scale  # Invert Y-axis for tkinter
                relative_segment.append((rel_x, rel_y))
            relative_coords.append(relative_segment)
        return relative_coords

    def draw_map(self):
        """Draw the map on the canvas and add the car model."""
        self.canvas.delete("all")  # Clear the canvas

        # Set the canvas background to black
        self.canvas.config(bg="black")

        # Draw the map's lines (same as before)
        for segment in self.relative_coords:
            for i in range(len(segment) - 1):
                x1, y1 = segment[i]
                x2, y2 = segment[i + 1]
                self.canvas.create_line(
                    self.center_pixel[0] + x1 * self.zoom_factor + self.offset[0],
                    self.center_pixel[1] + y1 * self.zoom_factor + self.offset[1],
                    self.center_pixel[0] + x2 * self.zoom_factor + self.offset[0],
                    self.center_pixel[1] + y2 * self.zoom_factor + self.offset[1],
                    fill="black", width=4
                )

        # Create a polygon (closed shape) with white color
        # You can adjust the coordinates to match your polygon shape
        polygon_coords = []
        for segment in self.relative_coords:
            for i in range(len(segment)):
                x, y = segment[i]
                polygon_coords.append(
                    (self.center_pixel[0] + x * self.zoom_factor + self.offset[0], 
                    self.center_pixel[1] + y * self.zoom_factor + self.offset[1])
                )

        # Draw the polygon and set the fill to white
        self.canvas.create_polygon(
            polygon_coords,
            fill="white", outline="black", width=2
        )



    def update_car_position(self, utm_x, utm_y, yaw):
        """Update the car's position and redraw the map."""
        self.car_utm = (utm_x, utm_y)
        self.car_yaw = yaw
        self.draw_map()

    def start_pan(self, event):
        """Record the starting position for panning."""
        self.start_x = event.x
        self.start_y = event.y

    def pan_map(self, event):
        """Handle panning of the map."""
        dx = event.x - self.start_x
        dy = event.y - self.start_y
        self.offset[0] += dx
        self.offset[1] += dy
        self.start_x = event.x
        self.start_y = event.y
        self.draw_map()

    def zoom_map(self, event):
        """Handle zooming of the map."""
        factor = 1.1 if event.delta > 0 else 0.9
        self.zoom_factor *= factor
        self.draw_map()

    def on_click(self, event):
        """Handle click on the canvas and convert the pixel to UTM coordinates."""
        # Get the pixel coordinates where the user clicked
        pixel_x = event.x - self.center_pixel[0] - self.offset[0]
        pixel_y = event.y - self.center_pixel[1] - self.offset[1]

        # Convert pixel coordinates back to UTM coordinates
        utm_x = self.center_utm[0] + pixel_x * self.scale * self.zoom_factor
        utm_y = self.center_utm[1] - pixel_y * self.scale * self.zoom_factor  # Invert Y-axis for UTM

        # Print the UTM coordinates
        print(f"UTM Coordinates: ({utm_x:.2f}, {utm_y:.2f})")
    def save_map_as_image(self):
        """Save the map as PNG and PGM files with black background and white polygon."""
        # Create a new image with a black background
        image = Image.new("RGB", self.canvas_size, "black")
        draw = ImageDraw.Draw(image)
        
        # Draw the lines (same as before)
        for segment in self.relative_coords:
            for i in range(len(segment) - 1):
                x1, y1 = segment[i]
                x2, y2 = segment[i + 1]
                draw.line(
                    [
                        (self.center_pixel[0] + x1 * self.zoom_factor + self.offset[0],
                        self.center_pixel[1] + y1 * self.zoom_factor + self.offset[1]),
                        (self.center_pixel[0] + x2 * self.zoom_factor + self.offset[0],
                        self.center_pixel[1] + y2 * self.zoom_factor + self.offset[1])
                    ],
                    fill="black", width=4
                )
        
        # Create a polygon with white fill (closed shape)
        polygon_coords = []
        for segment in self.relative_coords:
            for i in range(len(segment)):
                x, y = segment[i]
                polygon_coords.append(
                    (self.center_pixel[0] + x * self.zoom_factor + self.offset[0], 
                    self.center_pixel[1] + y * self.zoom_factor + self.offset[1])
                )

        # Draw the white polygon (closed shape)
        draw.polygon(polygon_coords, fill="white", outline="black")
        
        # Save the image as PNG
        image.save("map_single.png")
        
        # Convert the image to grayscale (L mode) for the PGM format
        image.convert("L").save("map_single.pgm")

    def save_map_metadata(self):
        """保存地图的元数据，包括基准站坐标"""
        metadata = {
            "map_size": self.canvas_size,
            "scale": self.scale,
            "center_utm": self.center_utm,
            "base_station_utm": self.base_station_utm  # 添加基准站坐标
        }
        with open("map_single.json", "w") as json_file:
            json.dump(metadata, json_file, indent=4)
            
            
    def calculate_map_corner_offset(self):
        # TODO
        """
        计算整张地图（画布）的左下角点相对于基准站的相对位置。
        """
        # 计算左下角点的像素坐标（画布范围）
        canvas_bottom_left_pixel_x = -self.center_pixel[0] - self.offset[0]
        canvas_bottom_left_pixel_y = self.canvas_size[1] - self.center_pixel[1] - self.offset[1]

        # 转换为 UTM 坐标
        bottom_left_utm_x = self.center_utm[0] + canvas_bottom_left_pixel_x * self.scale / self.zoom_factor
        bottom_left_utm_y = self.center_utm[1] - canvas_bottom_left_pixel_y * self.scale / self.zoom_factor

        # 计算相对基准站的偏移
        relative_x = bottom_left_utm_x - self.base_station_utm[0]
        relative_y = bottom_left_utm_y - self.base_station_utm[1]

        # 返回相对位置
        return relative_x, relative_y



async def websocket_listener(viewer, ws_url):
    """Asynchronous WebSocket listener to update the car's position in real-time."""
    async with websockets.connect(ws_url) as websocket:
        while True:
            message = await websocket.recv()
            if message.startswith("utm|"):
                _, easting, northing, yaw = message.split("|")
                viewer.update_car_position(float(easting), float(northing), float(yaw))


def start_websocket_listener(viewer, ws_url):
    """Start the asyncio event loop for the WebSocket listener."""
    loop = asyncio.new_event_loop()
    asyncio.set_event_loop(loop)
    loop.run_until_complete(websocket_listener(viewer, ws_url))

import math

def calculate_canvas_size(file_path, center_utm, scale, margin_pixels=10):
    """动态计算画布大小，确保地图完全显示并从中心向外扩展"""
    with open(file_path, 'r') as file:
        lines = file.readlines()

    # 找到所有的 UTM 坐标的最小和最大值
    utm_coords = []
    for line in lines:
        if line.strip():
            points = line.strip().split(',')
            utm_coords.extend([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])

    # 获取 X 和 Y 的最小值和最大值
    min_x = min(coord[0] for coord in utm_coords)
    max_x = max(coord[0] for coord in utm_coords)
    min_y = min(coord[1] for coord in utm_coords)
    max_y = max(coord[1] for coord in utm_coords)

    # 计算中心点到地图边缘的最大距离
    max_dist_x = max(abs(center_utm[0] - min_x), abs(center_utm[0] - max_x))
    max_dist_y = max(abs(center_utm[1] - min_y), abs(center_utm[1] - max_y))

    # 动态计算画布的像素大小，以确保地图完全显示
    width = (max_dist_x * 2) / scale
    height = (max_dist_y * 2) / scale
    # 加上边距
    width += 2 * margin_pixels
    height += 2 * margin_pixels
    # 向上取整到百的倍数
    width = math.ceil(width / 100) * 100
    height = math.ceil(height / 100) * 100



    # 返回画布大小
    return width, height




def calculate_center_utm(file_path):
    """动态计算地图中心点"""
    with open(file_path, 'r') as file:
        lines = file.readlines()

    # 找到所有的 UTM 坐标的最小和最大值
    utm_coords = []
    for line in lines:
        if line.strip():
            points = line.strip().split(',')
            utm_coords.extend([(float(points[i]), float(points[i + 1])) for i in range(0, len(points), 2)])

    # 获取 X 和 Y 的最小值和最大值
    min_x = min(coord[0] for coord in utm_coords)
    max_x = max(coord[0] for coord in utm_coords)
    min_y = min(coord[1] for coord in utm_coords)
    max_y = max(coord[1] for coord in utm_coords)

    # 计算中心点坐标
    center_x = (min_x + max_x) / 2
    center_y = (min_y + max_y) / 2

    # 返回中心点坐标
    return center_x, center_y


def main():
    # Parameters
    file_path = r"C:\Users\17963\Desktop\ros_workspace\navigation2_tutorials\my_test_dir\gps\utm_draw_map\utm_coordinates_single.txt"  # Replace with your file name
    # center_utm = (444431.969868, 4427046.542976)
    base_station_utm = (444431.969868, 4427046.542976)
    center_utm = calculate_center_utm(file_path)  # 动态计算地图中心点
    scale = 0.05  # 1 pixel = 0.05 meters
    # canvas_size = (5000, 5000)  # Canvas size in pixels
        # 计算动态画布大小
    canvas_size = calculate_canvas_size(file_path, center_utm, scale)
    ws_url = "ws://39.106.77.250:8089/ws?command_receiver&utm"  # Replace with your WebSocket server URL

    # Create Tkinter window
    root = tk.Tk()
    root.title("UTM Map Viewer")
    print(canvas_size)


    # Create MapViewer instance
    viewer = MapViewer(root, file_path, center_utm, scale, canvas_size, base_station_utm)
    offset_pixel = viewer.calculate_map_corner_offset()
    print(f"左下角点相对于基准站点的地图位置坐标（像素）: {offset_pixel}")
    viewer.save_map_as_image()
    viewer.save_map_metadata()
    # Start WebSocket listener in a separate thread
    threading.Thread(target=start_websocket_listener, args=(viewer, ws_url), daemon=True).start()

    # Run Tkinter loop
    root.mainloop()


if __name__ == "__main__":
    main()
